{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test BIT* Planner on Static Rand Maze2D Env with Concave Obstacles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time, sys\n",
    "sys.path.append('..')\n",
    "import numpy as np\n",
    "from importlib import reload\n",
    "import matplotlib.pyplot as plt\n",
    "from utils.maze2d_utils import plt_img, save_img\n",
    "from pb_diff_envs.environment.static.maze2d_rand_wgrp import Maze2DRandRecGroupList\n",
    "from pb_diff_envs.environment.static.rand_maze2d_env import RandMaze2DEnv\n",
    "\n",
    "no_while = True\n",
    "# --- 2 ---\n",
    "## avg one traj len: , horizon = \n",
    "num_groups = 5\n",
    "num_walls = 6\n",
    "maze_size = np.array([5, 5])\n",
    "hr = 0.50\n",
    "hExt_range = [hr, hr] # fixed-size obstacle\n",
    "seed = 333\n",
    "\n",
    "hExt_range = np.array(hExt_range)\n",
    "\n",
    "trivial = dict(gen_data=True, \n",
    "               samples_per_env=200, gen_num_parts=1, \n",
    "               eval_num_groups=6, eval_num_probs=10,\n",
    "               dataset_url='./dataset/luotest-randmaze2d-ipynb.hdf5')\n",
    "\n",
    "mazelist_config = dict(planner_timeout=30, \n",
    "                        interp_density=10,\n",
    "                        gap_betw_wall=0.15,\n",
    "                        min_to_wall_dist=0.02,\n",
    "                        )\n",
    "                        # min_episode_distance=)\n",
    "\n",
    "import gym\n",
    "env_name = 'randSmaze2d-C43-luotest-v0' # 100 prob: d8 (897, 2) -> (3459, 2)\n",
    "# env_name = 'randSmaze2d-C43-ng3ks25k-ms55nw21-hExt05-v0'\n",
    "# env_name = 'randSmaze2d-C43-ng3ks25k-ms55nw21-hExt05Rce01-v0'\n",
    "dvg_group = gym.make(env_name, load_mazeEnv=False, gen_data=True, is_eval=True) # True False\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "## some env group stats\n",
    "dvg_group.wallLoc_list.shape\n",
    "dvg_group.recs_grp.save_center_and_mode()\n",
    "dvg_group.recs_grp.centers_43[1]\n",
    "dvg_group.recs_grp.mode_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "## each concave obstacles consists of several small square blocks\n",
    "env = dvg_group.create_single_env(2)\n",
    "env.load(GUI=True)\n",
    "prev_pose = env.get_robot_free_pose()\n",
    "## solve motion planning task using BIT*\n",
    "solution, _ = env.sample_1_episode(prev_pose)\n",
    "print('solution', solution)\n",
    "env.robot.set_config(prev_pose)\n",
    "env.wall_locations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Vis 1 traj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.kuka_utils_luo import SolutionInterp\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "sol_interp = SolutionInterp(density=10)\n",
    "traj_vis = sol_interp(solution)\n",
    "print('solution', len(solution))\n",
    "solution_vis = np.array(solution)\n",
    "print('traj_vis', len(traj_vis))\n",
    "savepath = None\n",
    "img = env.render_1_traj(savepath, traj_vis[-70:],)\n",
    "plt_img(img)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Vis many trajs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.utils import DotDict\n",
    "from tqdm import tqdm\n",
    "from utils.kuka_utils_luo import SolutionInterp\n",
    "import matplotlib.pyplot as plt\n",
    "sol_interp = SolutionInterp(density=8)\n",
    "env.load(GUI=True) # False\n",
    "prev_pose = env.get_robot_free_pose()\n",
    "env.robot.set_config(prev_pose)\n",
    "solutions = []\n",
    "# n_traj = 25\n",
    "n_traj = 5\n",
    "for i in tqdm(range(n_traj)):\n",
    "    solution, _ = env.sample_1_episode(prev_pose)\n",
    "    solution = np.array(solution)\n",
    "    prev_pose = solution[-1]\n",
    "    solutions.append(solution)\n",
    "\n",
    "trajs = solutions\n",
    "solutions = np.concatenate(solutions, axis=0)\n",
    "result = DotDict(solution=solutions)\n",
    "\n",
    "\n",
    "print(solutions.shape)\n",
    "print(len(result.solution))\n",
    "# result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "solution = np.array(result.solution)\n",
    "# env.render_1_traj(solution)\n",
    "for i in tqdm(range(n_traj)):\n",
    "    trajs[i] = sol_interp(trajs[i])\n",
    "savepath = None\n",
    "tmp = env.render_composite(savepath, trajs)\n",
    "print(tmp.shape)\n",
    "# plt.clf()\n",
    "# plt.imshow(tmp)\n",
    "# plt.show()\n",
    "plt_img(tmp, dpi=200)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
